 classdef kalman2
    
    properties (SetAccess = private, GetAccess = public)
        
        % Filter properties
        N             % - iteration Mer (1 at zero time)
        M             % - number of track updates
        m             % - measurement vector length
        n             % - state vector length
        y             % - M-D position measurement (x, y, z)
        x_hat         % - N-D state vector estimate (x y z xd yd zd xdd ydd zdd)
        x_vec         % - history of state estimates
        K             % - Kalman gain vector
        L             % - % Initial state est population matrix from first measurement
        P             % - Filter covariance matrix
        P_pred        % - Predicted cov
        x_pred        % - Predicted pos
        P_fil_pos     % - Filter covariance matrix (position values only) 
        S             % - Residual cov matrix
        S_inv         % - Inverse resid cov matrix
        S_det         % - Determinant of residual cov matrix
        G_0           % - Gate according to eq 6.27, Blackman
        P_tru         % - True obs cov (debug only)
        P_D           % - Prob of det
        beta_t        % - total extraeneous target density
    end
    
    
    methods
        function o = kalman2(n, obs, PHI, H, Q, R, pos_var, v_max, L, kappa, P_D, beta_t)
            o.n       = n;
            o.m       = length(obs);
            % Incorporate first measurement
            o.y       = obs;
            o.x_hat   = L*o.y;
            o.P       = diag([pos_var (v_max/kappa).^2]);   % Initial filter covariance
            o.M       = 1;
            o.N       = 1;
            o.x_vec   = [];
            o.P_D     = P_D;
            o.beta_t  = beta_t;
            
            % Calculate prediction for next step
            o.x_pred = PHI*o.x_hat;
            o.P_pred = PHI*o.P*PHI'+ Q;
            o.S       = H*o.P_pred*H' + R; % Gate
            o.S_inv   = inv(o.S);
            o.S_det   = det(o.S);
            % Gate calc
            o.G_0     = 9; %2*log(o.P_D/((1-o.P_D)*(2*pi)^(o.m/2)*o.beta_t*sqrt(o.S_det)));
        end
            
        function o = iterate(o, pos_obs, PHI, H, Q, R)
            
            % Correction step
            o.y    = pos_obs;
            innov     = o.y-H*o.x_pred;
            o.K       = o.P_pred*H'*inv(o.S);
            o.x_hat   = o.x_pred + o.K*(innov);
            o.P       = (eye(o.n) - o.K*H)*o.P_pred*(eye(o.n) - o.K*H)' + o.K*R*o.K';
            
            % Prediction step (k+1)
            o.x_pred = PHI*o.x_hat;
            o.P_pred = PHI*o.P*PHI'+ Q;
            o.S       = H*o.P_pred*H' + R;
            o.S_inv   = inv(o.S);
            o.S_det   = det(o.S);
            % Gate calc
            %o.G_0     = 2*log(o.P_D/((1-o.P_D)*(2*pi)^(o.m/2)*o.beta_t*sqrt(o.S_det)));
            
            % Other 
            o.M       = o.M + 1;
            o.N       = o.N + 1;
            o.x_vec   = [o.x_vec o.x_hat];
            
            
        end
        
        function o = coast(o, PHI, H, Q, R, ibf) % add var for target in beam
            
            o.y       = NaN;
            % Correction step
            o.x_hat   = o.x_pred;
            o.P       = o.P_pred;
            if ibf
              o.N       = o.N + 1;
            end
            o.x_vec   = [o.x_vec o.x_hat];
            
            % Prediction step (k+1)
            o.x_pred = PHI*o.x_hat;
            o.P_pred = PHI*o.P*PHI'+ Q;
            o.S       = H*o.P_pred*H' + R; % Gate
            o.S_inv   = inv(o.S);
            o.S_det   = det(o.S);
            % Gate calc
            %o.G_0     = 2*log(o.P_D/((1-o.P_D)*(2*pi)^(o.m/2)*o.beta_t*sqrt(o.S_det)));
        end
        
   
        function out = is_in_gate(o, obs, H, beta)
            if o.N==0
                error('No prediction can be made - filter not iterated yet');
            else
                % Calc innovation
                innov     = obs-H*o.x_pred;
                if innov'*o.S_inv*innov<o.G_0
                    out = 1;
                else
                    out = 0;
                end
            end
        end
    end
end